#include <stdbool.h>
#include <stdint.h>

#include "../BSP/can.h"
#include "filter.h"
#include "pid.h"

#define MOTOR_CONTROL_ID (0x200)
#define MOTOR_FEEDBACK_ID (0x201)

#define MOTOR_MAX_CURRENT (16384)
#define MOTOR_MAX_ANGLE (8191.0f)

#define TIME (system_time_s + system_time_ms / 1000.0f)

typedef struct {
  float fb_angle;
  float fb_speed;
  int16_t fb_current;
  uint8_t fb_temperature;
} Motor_CAN_Feedback_t;

typedef struct {
  Motor_CAN_Feedback_t feedback;
  PID_t pid_speed;
  Filter_LowPass_t filter_speed;
  float out;
  float setpoint;
} Motor_t;

/**
 * @brief 初始化
 *
 */
void Motor_Init(void);

/**
 * @brief 电机输出
 *
 * @param value
 */
void Motor_Output(float value);

/**
 * @brief 解析反馈数据
 *
 */
void Motor_PraseData(void);

/**
 * @brief 控制电机
 *
 */
void Motor_Control(bool anti);

/**
 * @brief 返回目标转速
 *
 * @return float
 */
float Motor_GetSetpoint(void);

/**
 * @brief 返回反馈转速
 *
 * @return float
 */
float Motor_GetFeedback(void);
